#include "as5600.h"

void as5600_init(as5600_data_t *asdata,float lp_a)
{
	filter_init(&asdata->flp_angle_velocity,lp_a,12);
}

void as5600_angle_analytical(as5600_data_t *asdata,uint32_t sampling_time)
{
	asdata->angle = (asdata->read_data[0] << 8 | asdata->read_data[1]);
	asdata->last_angle_velocity = asdata->angle_velocity;
	
	int ev = asdata->angle - asdata->last_angle;
	if(abs(ev) < 2048)
	{
		asdata->angle_velocity = (asdata->angle - asdata->last_angle) * sampling_time;
//		if(abs(asdata->angle_velocity - asdata->last_angle_velocity) > 1500)
//		{
//			asdata->angle_velocity = asdata->last_angle_velocity;
//		}
	}
	asdata->last_angle = asdata->angle;
	
	filter_lp(&asdata->flp_angle_velocity,asdata->angle_velocity);
}
void as5600_status_analytical(as5600_data_t *asdata)
{
	uint8_t temp_data = (asdata->read_data[0] >> 3) & 0x07;
	asdata->status = temp_data;
}
